function [z,id_landmark_found]= get_observations(xtrue, landmark, id_landmark_found, rmax)

[landmark,id_landmark_found]= get_visible_landmarks(xtrue,landmark,id_landmark_found,rmax);
z= compute_range_bearing(xtrue,landmark);
%z = [];
%
%

function [landmark, id_landmark_found] = get_visible_landmarks(xtrue,landmark,id_landmark_found,rmax)
% Select set of landmarks that are visible within vehicle's semi-circular field-of-view
dx= landmark(1,:) - xtrue(1);
dy= landmark(2,:) - xtrue(2);
dz= landmark(3,:) - xtrue(3);
phi= pi_to_pi(xtrue(4));


% incremental tests for bounding semi-circle
ii= find(abs(dx) < rmax & abs(dy) < rmax & abs(dz) < rmax ... % bounding box
      & (dx*cos(phi) + dy*sin(phi)) > 0 ...  % bounding line
      & (dx.^2 + dy.^2) < rmax^2);           % bounding circle
% Note: the bounding box test is unnecessary but illustrates a possible speedup technique
% as it quickly eliminates distant points. Ordering the landmark set would make this operation
% O(logN) rather that O(N).




% incremental tests for bounding semi-circle
%ii= find(abs(dx) < rmax & abs(dy) < rmax & abs(dz) < rmax); % bounding box
% Note: the bounding box test is unnecessary but illustrates a possible speedup technique
% as it quickly eliminates distant points. Ordering the landmark set would make this operation
% O(logN) rather that O(N).




landmark= landmark(:,ii);
id_landmark_found = id_landmark_found(ii);

%
%

function z= compute_range_bearing(xtrue,landmark)
% Compute exact observation
dx= landmark(1,:) - xtrue(1);
dy= landmark(2,:) - xtrue(2);
dz= landmark(3,:) - xtrue(3);
z = [dx;
     dy;
     dz];
% 
% z = [landmark(1,:);
%      landmark(2,:);
%      landmark(3,:)];
% 
% rr = (xtrue(6)); % roll (x)
% pp = (xtrue(5)); % pitch (y)
% yy = (xtrue(4)); % yaw (z) 
%  
% R = [cos(yy)*cos(pp)    cos(yy)*sin(pp)*sin(rr)-sin(yy)*cos(rr)     cos(yy)*sin(pp)*cos(rr)+sin(yy)*sin(rr);
%      sin(yy)*cos(pp)    sin(yy)*sin(pp)*sin(rr)+cos(yy)*cos(rr)     sin(yy)*sin(pp)*cos(rr)-cos(yy)*sin(rr);
%      -sin(pp)           cos(pp)*sin(rr)                             cos(pp)*cos(rr)                        ];
% T = [xtrue(1);
%      xtrue(2);
%      xtrue(3)];
%  
%  % test
%  for i=1:size(z,2)
%      z(:,i) = (R')*z(:,i) - T;
%  end
% 
% z= [sqrt(dx.^2 + dy.^2);
%     atan2(dy,dx) - phi];
%     
